using BepuPhysics.Collidables;
using BepuUtilities;
using System;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.CollisionDetection.CollisionTasks
{
    // 单独的配对测试仪被设计为在狭窄阶段之外使用。它们需要可用于查询等,因此所有必要的数据都必须从外部收集。
    public struct SphereTriangleTester : IPairTester<SphereWide, TriangleWide, Convex1ContactManifoldWide>
    {
        public int BatchSize => 32;

        public void Test(ref SphereWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB,
            int pairCount, out Convex1ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        static void Select(ref Vector<float> distanceSquared, ref Vector3Wide localNormal, ref Vector<float> distanceSquaredCandidate, ref Vector3Wide localNormalCandidate)
        {
            var useCandidate = Vector.LessThan(distanceSquaredCandidate, distanceSquared);
            distanceSquared = Vector.Min(distanceSquaredCandidate, distanceSquared);
            Vector3Wide.ConditionalSelect(useCandidate, localNormalCandidate, localNormal, out localNormal);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Test(ref SphereWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount,
            out Convex1ContactManifoldWide manifold)
        {
            manifold = default;
            // 在三角形的局部空间中工作,因为变换球体位置比变换三角形的顶点更快。
            Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB);
            Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB);


            Vector3Wide.Subtract(b.B, b.A, out var ab);
            Vector3Wide.Subtract(b.C, b.A, out var ac);
            // localOffsetA=-localOffsetB,因此pa=triangle.A+localOffsetB。
            Vector3Wide.Add(b.A, localOffsetB, out var pa);
            Vector3Wide.CrossWithoutOverlap(ab, ac, out var localTriangleNormal);
            Vector3Wide.Dot(localTriangleNormal, pa, out var paN);
            var collidingWithSolidSide = Vector.GreaterThan(paN, Vector<float>.Zero);
            if (Vector.EqualsAll(collidingWithSolidSide, Vector<int>.Zero))
            {
                // 由于三角形的单边性,任何车道都不能产生接触。
                manifold.ContactExists = Vector<int>.Zero;
                return;
            }

            // EdgeAB平面测试：(Pa X Ab)*(Ab X Ac)>=0
            // EdgeAC平面测试：(Ac X Pa)*(Ab X Ac)>=0
            // 请注意,这些是重心坐标的缩放版本。
            // 要将它们规格化,使三角形内一个点的权重等于1,我们只需要除以点(ab x ac,ab x ac)。
            // 换句话说,要测试第三个边平面,我们可以确保未归一化的权重都是正的,并且和为小于点(ab x ac,ab x ac)的值。
            // 如果一个点在边缘平面之外,我们知道它不在面区域或任何其他边缘区域内。但是,它可能位于相邻的顶点区域。
            // 可以通过钳制边情况来处理顶点情况。
            // 此外,请注意,对于任何查询位置,即使该点位于两个边平面之外,也只测试一条边就足够了。如果它在两个边缘平面之外,
            // 这只是意味着它将位于共享顶点上,因此夹边测试可以捕获正确的最近点。
            // 因此,在每条边上,如果点在平面之外,请缓存该边。记录外部结果的最后一个边缘将被测试。
            // (Pa X Ab)*(Ab X Ac)=(pa*ab)*(ab*ac)-(pa*ac)*(ab*ab)
            // (Ac X Pa)*(Ab X Ac)=(ac*ab)*(pa*ac)-(ac*ac)*(pa*ab)
            // (Ab X Ac)*(Ab X Ac)=(ab*ab)*(ac*ac)-(ab*ac)*(ab*ac)
            Vector3Wide.Dot(pa, ab, out var abpa);
            Vector3Wide.Dot(ab, ac, out var abac);
            Vector3Wide.Dot(ac, pa, out var acpa);
            Vector3Wide.Dot(ac, ac, out var acac);
            Vector3Wide.Dot(ab, ab, out var abab);
            var edgePlaneTestAB = abpa * abac - acpa * abab;
            var edgePlaneTestAC = abac * acpa - acac * abpa;
            var triangleNormalLengthSquared = abab * acac - abac * abac;

            var edgePlaneTestBC = triangleNormalLengthSquared - edgePlaneTestAB - edgePlaneTestAC;
            var outsideAB = Vector.LessThan(edgePlaneTestAB, Vector<float>.Zero);
            var outsideAC = Vector.LessThan(edgePlaneTestAC, Vector<float>.Zero);
            var outsideBC = Vector.LessThan(edgePlaneTestBC, Vector<float>.Zero);

            var outsideAnyEdge = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideAC, outsideBC));
            Vector3Wide localClosestOnTriangle = default;
            var negativeOne = new Vector<int>(-1);
            if (Vector.EqualsAny(Vector.BitwiseAnd(collidingWithSolidSide, outsideAnyEdge), negativeOne))
            {
                // 至少有一条车道检测到三角形外的点。选择外部的一条边作为代表。
                Vector3Wide.ConditionalSelect(outsideAC, ac, ab, out var edgeDirection);
                Vector3Wide.Subtract(b.C, b.B, out var bc);
                Vector3Wide.ConditionalSelect(outsideBC, bc, edgeDirection, out edgeDirection);
                Vector3Wide.ConditionalSelect(outsideBC, b.B, b.A, out var edgeStart);

                Vector3Wide.Add(localOffsetB, edgeStart, out var negativeEdgeStartToP);
                // 如果边是AB或AC,这会做一些部分冗余的工作,但考虑到我们没有bcbc或bcpb,这是可以的。
                Vector3Wide.Dot(negativeEdgeStartToP, edgeDirection, out var negativeOffsetDotEdge);
                Vector3Wide.Dot(edgeDirection, edgeDirection, out var edgeDotEdge);
                var edgeScale = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, -negativeOffsetDotEdge / edgeDotEdge));
                Vector3Wide.Scale(edgeDirection, edgeScale, out var pointOnEdge);
                Vector3Wide.Add(edgeStart, pointOnEdge, out pointOnEdge);

                Vector3Wide.ConditionalSelect(outsideAnyEdge, pointOnEdge, localClosestOnTriangle, out localClosestOnTriangle);

            }
            if (Vector.EqualsAny(Vector.AndNot(collidingWithSolidSide, outsideAnyEdge), negativeOne))
            {
                // P+N*(pa*N)/||N||^2=N*(pa*N)/||N||^2-(-p)
                var nScale = paN / triangleNormalLengthSquared;
                Vector3Wide.Scale(localTriangleNormal, nScale, out var offsetToPlane);
                Vector3Wide.Subtract(offsetToPlane, localOffsetB, out var pointOnFace);

                Vector3Wide.ConditionalSelect(outsideAnyEdge, localClosestOnTriangle, pointOnFace, out localClosestOnTriangle);
            }

            manifold.FeatureId = Vector.ConditionalSelect(outsideAnyEdge, Vector<int>.Zero, new Vector<int>(MeshReduction.FaceCollisionFlag));

            // 我们将使用接触位置来执行边界平滑;为了找到其他三角形,接触位置必须位于网格曲面上。
            Matrix3x3Wide.TransformWithoutOverlap(localClosestOnTriangle, rB, out manifold.OffsetA);
            Vector3Wide.Add(manifold.OffsetA, offsetB, out manifold.OffsetA);
            Vector3Wide.Length(manifold.OffsetA, out var distance);
            // 请注意,法线已校准为从B指向A。
            var normalScale = new Vector<float>(-1) / distance;
            Vector3Wide.Scale(manifold.OffsetA, normalScale, out manifold.Normal);
            manifold.Depth = a.Radius - distance;
            // 如果球体的中心点与三角形接触,则法线未定义。在这种情况下,"正确的"法线应该是三角形的法线。
            // 但是,考虑到这是一种相当罕见的退化情况,并且我们已经将三角形背面视为无碰撞,我们将把零距离视为背面无碰撞。
            manifold.ContactExists = Vector.BitwiseAnd(
                Vector.GreaterThan(distance, Vector<float>.Zero),
                Vector.BitwiseAnd(
                    Vector.GreaterThanOrEqual(paN, Vector<float>.Zero),
                    Vector.GreaterThanOrEqual(manifold.Depth, -speculativeMargin)));
        }

        public void Test(ref SphereWide a, ref TriangleWide b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out Convex1ContactManifoldWide manifold)
        {
            throw new NotImplementedException();
        }
    }
}
